//
// Created by lustre on 2023/1/15.
//

#include "Quadruped.hpp"

Adafruit_PWMServoDriver *pwm = nullptr;


/**
 * 控制ticker每步具体动作，每个相邻的case 里内容经过ticker本身的延时分步执行。
 *
 * */
int step = -1;

void Quadruped::init(int sda, int scl) {
    Wire.setPins(sda, scl);

    pwm = new Adafruit_PWMServoDriver(PCA9685_I2C_ADDRESS, Wire);
    pwm->begin();
    pwm->setPWMFreq(PWM_FREQUENCY);
    // 关节复位
    for (Joint *joint: joints) {
        pwm->setPWM(joint->pin, 0, joint->resetPos);
    }
}



/**
 * 每一步动作需要舵机转到对应的角度后才能进行下一个动作否则
 * 会导致直接忽略中间的动作，因而需要在同一个舵机两个转动之间delay <br>
 * 而由Ticker执行时不能使用delay函数，但Ticker每次执行是有延时的，将Ticker的间隔设为舵机转动需要的延时
 * step变量指向每步的动作
 *
 * */
// 左腿右迈左蹬 右腿左迈右蹬
void Quadruped::forward() {

    step = ++step % 6;
    switch (step) {
        case 0:
            frontLeftKnee.up();
            rightRearKnee.up();
            frontLeftLeg.right();
            rightRearLeg.left();
            return;
        case 1:
            frontLeftKnee.down();
            rightRearKnee.down();
            rightFrontKnee.up();
            rearLeftKnee.up();
            return;
        case 2:
            frontLeftLeg.left();
            rightRearLeg.right();
            return;
        case 3:
            rightFrontLeg.left();
            rearLeftLeg.right();
            return;
        case 4:
            rearLeftKnee.down();
            rightFrontKnee.down();
            frontLeftKnee.up();
            rightRearKnee.up();
            return;
        case 5:
            rightFrontLeg.right();
            rearLeftLeg.left();
            return;
    }

}

// 左腿右迈左蹬 右腿左迈右蹬
void Quadruped::fastForward() {
    step = ++step % 4;
    switch (step) {
        case 0:
            frontLeftKnee.up();
            rightRearKnee.up();
            // 悬空摆动
            frontLeftLeg.right();
            rightRearLeg.left();
            return;
        case 1:
            frontLeftKnee.down();
            rightRearKnee.down();
            // 交换脚，使得落脚获得摩擦力
            rightFrontKnee.up();
            rearLeftKnee.up();
            // 蹬腿摆动
            frontLeftLeg.left();
            rightRearLeg.right();
            return;
        case 2:

            // 悬空摆动
            rearLeftKnee.up();
            rearLeftKnee.up();
            rightFrontLeg.left();
            rearLeftLeg.right();
            return;
        case 3:
            rearLeftKnee.down();
            rightFrontKnee.down();
            // 交换脚，使得落脚获得摩擦力
            frontLeftKnee.up();
            rightRearKnee.up();
            // 蹬腿摆动
            rightFrontLeg.right();
            rearLeftLeg.left();
            return;
    }

}

// 右腿右迈左蹬 左腿左迈右蹬
void Quadruped::backward() {
    step = ++step % 6;
    switch (step) {
        case 0:
            frontLeftKnee.up();
            rightRearKnee.up();
            frontLeftLeg.left();
            rightRearLeg.right();
            return;
        case 1:
            rightFrontKnee.up();
            rearLeftKnee.up();
            frontLeftKnee.down();
            rightRearKnee.down();
            return;
        case 2:
            frontLeftLeg.right();
            rightRearLeg.left();
            return;
        case 3:
            rightFrontLeg.right();
            rearLeftLeg.left();
            return;
        case 4:
            frontLeftKnee.up();
            rightRearKnee.up();
            rearLeftKnee.down();
            rightFrontKnee.down();
            return;
        case 5:
            rightFrontLeg.left();
            rearLeftLeg.right();
            return;
    }


}

// 左腿右迈左蹬 右腿右迈左蹬
void Quadruped::turnRight() {
    step = ++step % 6;
    switch (step) {
        case 0:
            frontLeftKnee.up();
            rightRearKnee.up();
            frontLeftLeg.right();
            rightRearLeg.right();
            return;
        case 1:
            frontLeftKnee.down();
            rightRearKnee.down();
            rightFrontKnee.up();
            rearLeftKnee.up();
            return;
        case 2:
            frontLeftLeg.left();
            rightRearLeg.left();
            return;
        case 3:
            rightFrontKnee.up();
            rearLeftKnee.up();
            rightFrontLeg.right();
            rearLeftLeg.right();
            return;
        case 4:
            rearLeftKnee.down();
            rightFrontKnee.down();
            frontLeftKnee.up();
            rightRearKnee.up();
            return;
        case 5:
            rightFrontLeg.left();
            rearLeftLeg.left();
            return;
    }
}

// 左腿左迈右蹬 右腿左迈右蹬
void Quadruped::turnLeft() {
    step = ++step % 6;
    switch (step) {
        case 0:
            frontLeftKnee.up();
            rightRearKnee.up();
            frontLeftLeg.left();
            rightRearLeg.left();
            return;
        case 1:
            frontLeftKnee.down();
            rightRearKnee.down();
            rightFrontKnee.up();
            rearLeftKnee.up();
            return;
        case 2:
            frontLeftLeg.right();
            rightRearLeg.right();
            return;
        case 3:
            rightFrontLeg.left();
            rearLeftLeg.left();
            return;
        case 4:
            rearLeftKnee.down();
            rightFrontKnee.down();
            frontLeftKnee.up();
            rightRearKnee.up();
            return;
        case 5:
            rightFrontLeg.right();
            rearLeftLeg.right();
            return;
    }
}


void Quadruped::stop() {
    this->quadrupedAction = -1;
}

void Quadruped::reset() {
    stop();
    for (Joint *joint: joints) {
        pwm->setPWM(joint->pin, 0, joint->resetPos);
    }
}

void Quadruped::turn(uint8_t target, uint16_t pos) {
    Serial.print("target:");
    Serial.print(target);
    Serial.print("src_pos:");
    Serial.print(pos);
    pos = mappingToPwmValue(pos);
    Serial.print(",pos:");
    Serial.println(pos);
    pwm->setPWM(target, 0, pos);
}

void Quadruped::doAsyncAction(int action) {
    void (Quadruped::*anAction)() = actionTab[action];
    (this->*anAction)();
}

volatile int Quadruped::getQuadrupedAction() const {
#ifdef ACTION_DEBUG
    Serial.printf("get action %d\n", quadrupedAction);
#endif

    return quadrupedAction;
}

void Quadruped::setQuadrupedAction(volatile int action) {
#ifdef ACTION_DEBUG
    Serial.printf("set action %d", action);
#endif
    // 将下一步重置为0
    step = -1;
    Quadruped::quadrupedAction = action;
    // 重新计算限位复位特殊动作对限位的修改
    setStepLength(stepLength);
    setStepHigh(stepHigh);
}

void Quadruped::sitDown() {
    setStepLengthOnce(50);
    setStepHigh(40);
    // 将后腿收往侧前方
    rearLeftLeg.right();
    rightRearLeg.left();
    rearLeftKnee.up();
    rightRearKnee.up();

    frontLeftLeg.right();
    rightFrontLeg.left();
    frontLeftKnee.down();
    rightFrontKnee.down();
}

void Quadruped::setStepHigh(uint8_t rate) {
    this->stepHigh = rate;
    setStepHighOnce(rate);
}

void Quadruped::setStepLength(uint8_t rate) {
    this->stepLength = rate;
    setStepLengthOnce(rate);
}

void Quadruped::setLeftRightGimbalPos(uint8_t pos) {
#if defined(LEFT_RIGHT_GIMBAL_PIN) && LEFT_RIGHT_GIMBAL_PIN > -1
    turn(LEFT_RIGHT_GIMBAL_PIN, pos);
#endif
}

void Quadruped::setUpDownGimbalPos(uint8_t pos) {
#if defined(UP_DOWN_GIMBAL_PIN) && UP_DOWN_GIMBAL_PIN > -1
    turn(UP_DOWN_GIMBAL_PIN, pos);
#endif
}

void Quadruped::setYawRate(uint8_t rate) {
    int rate_ = rate - 100;
    if (rate_ == this->yawRate) {
        return;
    }
    this->yawRate = rate_;

    // 复位
    if (rate_ == 0) {
        setStepLength(stepLength);
        return;
    }
    // 拖动条在中点右边，右转则需要收缩右边的步长
    if (rate_ > 0) {
        int yawStepLength = stepLength * (100 - rate_) / 100;
        rightFrontLeg.setStepLength(yawStepLength);
        rightRearLeg.setStepLength(yawStepLength);
        frontLeftLeg.setStepLength(stepLength);
        rearLeftLeg.setStepLength(stepLength);
    } else {
        int yawStepLength = stepLength * (100 + rate_) / 100;
        frontLeftLeg.setStepLength(yawStepLength);
        rearLeftLeg.setStepLength(yawStepLength);
        rightFrontLeg.setStepLength(stepLength);
        rightRearLeg.setStepLength(stepLength);
    }
}

void Quadruped::setStepHighOnce(uint8_t rate) {
    frontLeftKnee.setStepHigh(rate, kneeConvergence);
    rightFrontKnee.setStepHigh(rate, kneeConvergence);
    rightRearKnee.setStepHigh(rate, kneeConvergence);
    rearLeftKnee.setStepHigh(rate, kneeConvergence);
}

void Quadruped::setStepLengthOnce(uint8_t rate) {
    frontLeftLeg.setStepLength(rate);
    rightFrontLeg.setStepLength(rate);
    rightRearLeg.setStepLength(rate);
    rearLeftLeg.setStepLength(rate);
}

Quadruped::Quadruped(const QuadrupedVariantConf &variantConf) :
        frontLeftLeg(0, variantConf, 0),
        frontLeftKnee(1, variantConf, 1),
        rightFrontLeg(2, variantConf, 2),
        rightFrontKnee(3, variantConf, 3),
        rightRearLeg(4, variantConf, 4),
        rightRearKnee(5, variantConf, 5),
        rearLeftLeg(6, variantConf, 6),
        rearLeftKnee(7, variantConf, 7),
        kneeConvergence(variantConf.kneeConvergence) {
    setStepHigh(variantConf.stepHigh);
    setStepLength(variantConf.stepLength);
}

void Quadruped::turnLimit(uint8_t target, bool start) {
    if (target > 7) {
        return;
    }
    if (start) {
        joints[target]->turnStart();
    } else {
        joints[target]->turnEnd();
    }

}


KneeJoint::KneeJoint(int pin, const QuadrupedVariantConf &quadrupedVariantConf, int flagIndex) :
        Joint(
                pin,
                quadrupedVariantConf.upperLimit,
                quadrupedVariantConf.lowerLimit,
                quadrupedVariantConf.kneeResetPos,
                quadrupedVariantConf.direction[flagIndex]
        ) {
    this->realLowerLimit = mappingToPwmValue(lowerLimit);
    this->realUpperLimit = mappingToPwmValue(upperLimit);

}

void KneeJoint::up() {
    log_i("%d up", this->pin);
    pwm->setPWM(this->pin, 0, this->nowUpper);
}

void KneeJoint::down() {
    log_i("%d down", this->pin);
    pwm->setPWM(this->pin, 0, this->nowLower);
}

/**
 * 提腿高度 向着远离 100 %方向收缩，或中位线收缩
 * */
void KneeJoint::setStepHigh(uint8_t rate, KneeConvergence kneeConvergence) {
    if (rate < 1 || rate > 100) {
        return;
    }
    if (kneeConvergence == MIDPOINT_CONVERGENCE) {
        int mid = (realUpperLimit + realLowerLimit) / 2;
        int diff = (realUpperLimit - realLowerLimit) * rate / 200;
        this->nowLower = mid - diff;
        this->nowUpper = mid + diff;
    } else if (kneeConvergence == LEFT_CONVERGENCE) {
        if (lowerLimit < upperLimit) {
            this->nowUpper = (END_POS - BASE_POS) * upperLimit * rate / 10000 + BASE_POS;
            this->nowLower = (END_POS - BASE_POS) * lowerLimit * rate / 10000 + BASE_POS;
        } else {
            this->nowUpper = END_POS - (END_POS - BASE_POS) * (100 - upperLimit) * rate / 10000;
            this->nowLower = END_POS - (END_POS - BASE_POS) * (100 - lowerLimit) * rate / 10000;
        }
    }

    log_i("pin:%d , nowUpper:%d, nowLower:%d", pin, nowUpper, nowLower);
}


Joint::Joint(int pin, int upperLimit, int lowerLimit, int resetPos, bool direction) :
        pin(pin),
        upperLimit(direction ? upperLimit : lowerLimit),
        lowerLimit(direction ? lowerLimit : upperLimit),
        resetPos(mappingToPwmValue((direction ? resetPos : (100 - resetPos)))) {}


HipJoint::HipJoint(int pin, const QuadrupedVariantConf &quadrupedVariantConf, int flagIndex) :
        Joint(
                pin,
                quadrupedVariantConf.rightLimit,
                quadrupedVariantConf.leftLimit,
                quadrupedVariantConf.hipResetPos,
                quadrupedVariantConf.direction[flagIndex]
        ) {
    this->realLeftLimit = mappingToPwmValue(upperLimit);
    this->realRightLimit = mappingToPwmValue(lowerLimit);
}

void HipJoint::left() {
    log_i("%d left", this->pin);
    pwm->setPWM(this->pin, 0, this->leftWalkingLimit);
}

void HipJoint::right() {
    log_i("%d right", this->pin);
    pwm->setPWM(this->pin, 0, this->rightWalkingLimit);
}

/**
 * 步长，这里对称限位范围的中位线进行收缩
 * @param 1 ~ 100
 * */
void HipJoint::setStepLength(uint8_t rate) {
    // 区间 按rate%比例 并沿着中位线 对称收缩
    int mid = (realLeftLimit + realRightLimit) / 2; // 计算中位数
    int diff = (realRightLimit - realLeftLimit) * rate / 200;
    this->leftWalkingLimit = mid - diff;
    this->rightWalkingLimit = mid + diff;
}